/*
 * Seeker.cpp
 *
 *  Created on: Jun 11, 2011
 *      Author: wilsok17
 */

#include "Seeker.h"
#include "../LFOS/LFOSC2Interface.h"
#include "../LFOS/CameraInterface.h"
#include "../Yogurt/Grid.h"
#include "../Yogurt/Search.h"
#include "../Barf/SUDSC2Intf.h"
#include "../Yogurt/utility.h"
#include <cstdlib>
#include <iostream>
#if DEBUG
#include <sstream>
#include <fstream>
#endif

#if DEBUG
void dumpFrame(unsigned char* frame, int frameNum);
#endif


Seeker::Seeker(int id) {
	seekerID_ = id;
	currentStep_ = 0;
}

Seeker::~Seeker() {

}

void Seeker::WaitForButton(){
	char readBuf[6];
	int n, fd;
    fd = open("/dev/buttons", O_RDONLY);

    cout << "Waiting for Button 1 press" << endl;
    if(fd != 0)
    {
    	n = read(fd, readBuf, 6);
        if (n != -1){
    		while(readBuf[0] != 49)
    		{
    	    	n = read(fd, readBuf, 6);
    		}
        }
    }
    close(fd);
	cout << "Done Waiting" << endl;
}

void Seeker::run() {
	WaitForButton();
	cout << "Initializing seeker components needed for run()" << endl;
	uint8_t speed = 50;
	LFOSC2Interface lfos;
	CameraInterface camera;
	camera.Initialize(250);
	DetectionInfo info;
	unsigned char* frame;
#if !SIM
	 frame = camera.CaptureFrame();
#endif
//	if (frame != 0) {
//		info = lfos.ProcessImage(frame);
//	}
	cout << "Initializing grid" << endl;
	Grid grid;
	SUDSC2Intf suds;
	Search search;
	list<pair<string, DIRECTION> > route = search.findPath(grid, "A", "AK");
	receiveInstructions(route);
	currentNode_ = "A";
	cout << "Begin grid traversal" << endl;
	cout << "Trying first route: " << endl;
	list<pair<string, DIRECTION> >::iterator i;
#if DEBUG
	int frameNum = 0;
#endif

	for (i = route.begin(); i != route.end(); i++) {
		cout << "Node:" << i->first << "  Direction:" << grid.directionToString(i->second) << endl;
	}
	cout << endl;
	while (currentNode_.compare("AK") != 0) {
		for (i = route.begin(); i != route.end(); ++i) {
			cout << "Check path from " << currentNode_ << " to " << i->first
					<< endl;
			DIRECTION dir = i->second;
			//send dir to MCS and move seeker
			switch (dir) {
			case START:
				mcs_.MoveForwardAtSpeed(speed);
				break;
			case N:
				switch (currentDir_) {
				case START:
					mcs_.TurnLeft(45);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case N:
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case S:
					mcs_.TurnLeft(180);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case E:
					mcs_.TurnRight(90);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case W:
					mcs_.TurnLeft(90);
					mcs_.MoveForwardAtSpeed(speed);
				}
				break;
			case S:
				switch (currentDir_) {
				case START:
					mcs_.TurnRight(45);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case N:
					mcs_.TurnLeft(180);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case S:
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case E:
					mcs_.TurnLeft(90);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case W:
					mcs_.TurnRight(90);
					mcs_.MoveForwardAtSpeed(speed);
				}
				break;
			case E:
				switch (currentDir_) {
				case START:
					mcs_.TurnRight(45);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case N:
					mcs_.TurnRight(90);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case S:
					mcs_.TurnLeft(90);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case E:
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case W:
					mcs_.TurnLeft(180);
					mcs_.MoveForwardAtSpeed(speed);
				}
				break;
			case W:
				switch (currentDir_) {
				case START:
					mcs_.TurnLeft(45);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case N:
					mcs_.TurnLeft(90);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case S:
					mcs_.TurnRight(90);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case E:
					mcs_.TurnLeft(180);
					mcs_.MoveForwardAtSpeed(speed);
					break;
				case W:
					mcs_.MoveForwardAtSpeed(speed);
				}
				break;
			}
			currentDir_ = dir;

			frame = camera.CaptureFrame();

			info.type = 0;
			if (frame != 0) {
#if DEBUG
				dumpFrame(frame, frameNum++);
#endif
				info = lfos.ProcessImage(frame);
			}
			else
			{
				info.type = 0;
			}
			cout << info.type << endl;
			cout << info.drivewayIntersection << endl;
			cout << "Angle: " << info.angle << ", Dist: " << info.fromCenter << endl;

			bool foundIED = false;
			bool foundBreak = false;
			//run until next intersection
			while ((info.type & IntersectionReached) == 0) {
				frame = camera.CaptureFrame();
				if (frame != 0) {
#if DEBUG
					dumpFrame(frame, frameNum++);
#endif
					cout << "Waiting to find intersection" << endl;
					info = lfos.ProcessImage(frame);
					cout << "Angle: " << info.angle << ", Dist: " << info.fromCenter << endl;
					cout << "New Info.type:" << info.type << endl;
					if(foundIED || foundBreak) {
						mcs_.MoveForwardAtSpeed(speed);
					}
				} else {
					info.type = 0;
				}

				if((info.type & Obstruction) != 0) {
					cout << "Obstruction Found, reversing direction";
					mcs_.Stop();
					mcs_.MoveBackwardAtSpeed(speed);
					wait(500);
					mcs_.TurnLeft(180);
					//mcs_.MoveForwardAtSpeed(speed);
					currentDir_ = grid.getOppositeDirection(currentDir_);
					grid.removeTwoWayEdge(currentNode_, *i);
					foundBreak = true;
				}

				int detection = suds.checkDetections();
				if (detection != 0) {
					cout << "IED found, reversing direction" << endl;
					//tell mcs to turn around
					mcs_.Stop();
					mcs_.MoveBackwardAtSpeed(speed);
					wait(500);
					mcs_.TurnLeft(180);
					//mcs_.MoveForwardAtSpeed(speed);
					currentDir_ = grid.getOppositeDirection(currentDir_);
					grid.removeTwoWayEdge(currentNode_, *i);
					foundIED = true;
				}
			}

			cout << "Intersection found" << endl;

			mcs_.Stop();
			if (foundIED || foundBreak) {
				break;
			} else {
				currentNode_ = i->first;
			}
		}
		cout << endl << endl << "Trying new route: " << endl;
		route = search.findPath(grid, currentNode_, "AK");
		for (i = route.begin(); i != route.end(); i++) {
			cout << "Node:" << i->first << "  Direction:" << grid.directionToString(i->second) << endl;
		}
		cout << endl;

	}
	mcs_.Stop();
	//	while(true) {
	//		Message msg = schwartz.retrieveIncomming();
	//		if(msg.type_ == 2) {
	//			if(msg.seekerID_ == seekerID_) {
	//				SeekerRoute* route = static_cast<SeekerRoute*>(&msg);
	//				receiveInstructions(route->rtDirections_);
	//			}
	//		}
	// 	}
}

SeekerReport Seeker::reportIntersection() {
	SeekerReport report;
	report.lastRouteLegComplete_ = currentStep_;
	report.pathStatus_ = 0;
	report.seekerID_ = seekerID_;
	++currentStep_;
	report.currentLeg_ = currentStep_;
	//schwartz_.sendReport(report);
	return report;
}

SeekerReport Seeker::reportIED() {
	SeekerReport report;
	report.lastRouteLegComplete_ = currentStep_;
	report.currentLeg_ = currentStep_;
	report.pathStatus_ = 1;
	report.seekerID_ = seekerID_;
	//schwartz_.sendReport(report);
	return report;
}

SeekerReport Seeker::reportObstruction() {
	SeekerReport report;
	report.lastRouteLegComplete_ = currentStep_;
	report.currentLeg_ = currentStep_;
	report.pathStatus_ = 2;
	report.seekerID_ = seekerID_;
	//schwartz_.sendReport(report);
	return report;
}

//void Seeker::receiveInstructions(list<DIRECTION> seekerRoute) {
//	seekerRoute_ = seekerRoute;
//	currentStep_ = 0;
//}

void Seeker::receiveInstructions(list<pair<string, DIRECTION> > seekerRoute) {
	seekerRoute_ = seekerRoute;
	currentStep_ = 1;
}

#if DEBUG
void dumpFrame(unsigned char* frame, int frameNum)
{
	stringstream ss;
	ofstream output;
	ss << "frame_";
	ss << frameNum;
	ss << ".txt";

	output.open(ss.str().c_str());

	for(int y = 0; y < ROWS; ++y)
	{
		output << (int)frame[ROW_COL_TO_INDEX(0,y)];
		for(int x = 1; x < COLS; ++x)
		{
			output << ", "<< (int)frame[ROW_COL_TO_INDEX(x,y)];
		}
		output << endl;
	}

	output.close();
}
#endif

